package com.bsj.power.common.util;

import cn.hutool.core.util.NumberUtil;
import com.alibaba.fastjson.JSON;
import com.alibaba.fastjson.JSONArray;
import com.alibaba.fastjson.JSONObject;
import lombok.extern.slf4j.Slf4j;

/**
 * @author yinhao
 * @version 1.0
 * @description 位置坐标系
 * @date 2024/10/29
 */
@Slf4j
public class PosUtil {

    // 经纬度原点
//    private static final GlobalCoordinates origin = new GlobalCoordinates(36.70981, 121.378609);
    private static final double stdlon = 121.378609 * Math.PI / 180;
    private static final double stdlat = 36.70981 * Math.PI / 180;
    private static final double stdalt = 8.3;
    private static final double EARTH_RADIUS = 6378.137; // 地球半径，单位km

    /**
     * 根据经纬度获取xy
     * @param lon
     * @param lat
     * @param alt
     * @return
     */
    public static double[] AssessPos(double lon, double lat, double alt) {
        lon = lon * Math.PI / 180;
        lat = lat * Math.PI / 180;

        //计算位置偏差
        double[] xyz1 = lla2xyz(stdlon, stdlat, stdalt);
        double[] xyz2 = lla2xyz(lon, lat, alt);
        double deltaX = xyz2[0] - xyz1[0];
        double deltaY = xyz2[1] - xyz1[1];
        double deltaZ = xyz2[2] - xyz1[2];

        //参考经度
        double lon1 = stdlon;
        //参考纬度
        double lat1 = stdlat;
        //参考高度
        double alt1 = stdalt;

        double cos_lat1 = Math.cos(lat1);
        double sin_lat1 = Math.sin(lat1);
        double cos_lon1 = Math.cos(lon1);
        double sin_lon1 = Math.sin(lon1);

        double[] enu = new double[4];
        //东
        enu[0] = cos_lon1 * deltaY - sin_lon1 * deltaX;
        //北
        enu[1] = cos_lat1 * deltaZ - sin_lat1 * cos_lon1 * deltaX - sin_lat1 * sin_lon1 * deltaY;
        //天
        enu[2] = cos_lat1 * cos_lon1 * deltaX + cos_lat1 * sin_lon1 * deltaY + sin_lat1 * deltaZ;
        enu[3] = Math.sqrt(enu[0] * enu[0] + enu[1] * enu[1]);

        return xyzDidOffset(enu);
    }

    /// <summary>
    /// 将大地坐标转换到ECEF直角坐标
    /// </summary>
    /// <param name="lon">大地坐标(经度lon)单位：弧度</param>
    /// <param name="lat">纬度lat</param>
    /// <param name="alt">高度alt</param>
    /// <returns>ECEF直角坐标(x,y,z)，单位：m</returns>
    public static double[] lla2xyz(double lon, double lat, double alt) {
        double Ra = 6378137;
        double EC2 = 6.694379990146e-3;      //
        //double Ra = 6378136.3;              //
        //double EC2 = 6.69437999014e-3;      //
        double dlon = lon;
        double dlat = lat;
        double dalt = alt;
        double sinlat = Math.sin(dlat);
        double coslat = Math.cos(dlat);
        double sinlon = Math.sin(dlon);
        double coslon = Math.cos(dlon);

        //计算卯酉圈半径rn
        double rn = Ra / Math.sqrt(1 - EC2 * sinlat * sinlat);

        //计算直角坐标下x
        double x = (rn + dalt) * coslat * coslon;

        //计算直角坐标下y
        double y = (rn + dalt) * coslat * sinlon;

        //计算直角坐标下x
        double z = (rn * (1 - EC2) + dalt) * sinlat;

        double[] _mResult = new double[3];
        _mResult[0] = x;
        _mResult[1] = y;
        _mResult[2] = z;
        return _mResult;
    }

    /**
     * 需要进行一些偏移
     * @param pos
     * @return
     */
    public static double[] xyzDidOffset(double[] pos) {
        //东
        double east = pos[0];
        //北
        double north = pos[1];
        //X=东*COS(6.4)-北*SIN(6.4)
        //y=东*SIN(6.4)-北*COS(6.4)
        double[] xyz = new double[3];
        xyz[0] = east * Math.cos(6.4) - north * Math.sin(6.4);
        xyz[1] = east * Math.sin(6.4) - north * Math.cos(6.4);
        return xyz;
    }

    /**
     * 判断点是否在路线上
     * @param pathLineData 路线
     * @param pathMapId 该路线的空间坐标
     * @param radius 可偏差值
     * @param x 待判断点x
     * @param y 待判断点y
     * @param mapId 待判断点的空间
     * @return
     */
    public static boolean posInRoad(String pathLineData, Integer pathMapId, Double radius,
                                    double x, double y, Integer mapId) {
        if (!pathMapId.equals(mapId)) {
            return false;
        }
        JSONArray jsonArray = JSON.parseArray(pathLineData);
        if (jsonArray.isEmpty()) {
            return false;
        }
        int size = jsonArray.size();
        Double minRoad = null;
        for (int i = 0; i < jsonArray.size(); i++) {
            if (i == size - 1) {
                //每次需要比较不同的路线
                break;
            }
            JSONObject startObj = jsonArray.getJSONObject(i);
            Double x1 = NumberUtil.round(startObj.getDouble("x"), 2).doubleValue();
            Double y1 = NumberUtil.round(startObj.getDouble("z"), 2).doubleValue();
            JSONObject endObj = jsonArray.getJSONObject(i + 1);
            Double x2 = NumberUtil.round(endObj.getDouble("x"), 2).doubleValue();
            Double y2 = NumberUtil.round(endObj.getDouble("z"), 2).doubleValue();
            //待比较点到路线的最短距离
            double line = pointToLine(x1, y1, x2, y2, NumberUtil.round(x, 2).doubleValue(), NumberUtil.round(y, 2).doubleValue());
            if (minRoad == null) {
                minRoad = line;
            } else {
                if (minRoad > line) {
                    minRoad = line;
                }
            }
        }
        if (minRoad <= radius) {
            return true;
        }
//        log.warn("距离过远：{}", minRoad);
        return false;
    }

    /**
     * 点到直线的最短距离的判断
     * @param x1 由两点组成的线段 x1
     * @param y1 由两点组成的线段 y1
     * @param x2 由两点组成的线段 x2
     * @param y2 由两点组成的线段 y2
     * @param x0 待比较的点x
     * @param y0 待比较的点y
     * @return
     */
    public static double pointToLine(double x1, double y1, double x2, double y2, double x0, double y0) {
        double space = 0;
        double a, b, c;

        a = GetDistance(x1, y1, x2, y2);// 线段的长度
        b = GetDistance(x1, y1, x0, y0);// (x1,y1)到点的距离
        c = GetDistance(x2, y2, x0, y0);// (x2,y2)到点的距离

        /*
         * logger.info("--x0:" + x0 + "--y0:"+y0); logger.info("--x1:" + x1 +
         * "--y1:"+y1); logger.info("--x2:" + x2 + "--y2:"+y2);
         */
        // 点在线段的两端，即在线段上
        if (c <= 0.000001 || b <= 0.000001) {
            space = 0;
            return space;
        }

        // 线段是一个点，即转化为两点之间的距离
        if (a <= 0.000001) {
            space = b;
            return space;
        }

        // 组成直角三角形或钝角三角形，(x1,y1)为直角或钝角 ,那么点到(x2,y2)的一点是最近的一点
        if (c * c >= a * a + b * b) {
            space = b;
            return space;
        }

        // 组成直角三角形或钝角三角形，(x2,y2)为直角或钝角，那么点到(x1,y1)的一点是最近的一点
        if (b * b >= a * a + c * c) {
            space = c;
            return space;
        }

        // 组成锐角三角形，则求三角形的高
        double p = (a + b + c) / 2;// 半周长
        double s = Math.sqrt(p * (p - a) * (p - b) * (p - c));// 海伦公式求面积

        space = 2 * s / a;// 返回点到线的距离（利用三角形面积公式求高）

        return space;
    }

    private static double GetDistance(double x1, double y1, double x2, double y2) {
        double lineLength = 0;

        lineLength = Math.sqrt((x1 - x2) * (x1 - x2) + (y1 - y2)

                * (y1 - y2));

        return lineLength;
    }

//    private static double GetDistance(double lat1, double lng1, double lat2, double lng2) {
//        double radLat1 = rad(lat1);
//        double radLat2 = rad(lat2);
//        double a = radLat1 - radLat2;
//        double b = rad(lng1) - rad(lng2);
//        double s = 2 * Math.asin(Math.sqrt(
//                Math.pow(Math.sin(a / 2), 2) + Math.cos(radLat1) * Math.cos(radLat2) * Math.pow(Math.sin(b / 2), 2)));
//        s = s * EARTH_RADIUS * 1000;
//        s = Math.round(s * 10000) / 10000;
//        return s;
//    }

    private static double rad(double d) {
        return d * Math.PI / 180.0;
    }

    public static void main(String[] args) {
        double[] pos = AssessPos(121.3784, 36.71002, 2);
        System.out.println(JSON.toJSONString(pos));
    }

}
